
/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */

package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.AnalogChannel;
import edu.wpi.first.wpilibj.DigitalInput;
import edu.wpi.first.wpilibj.Solenoid;
import edu.wpi.first.wpilibj.SpeedController;
import java.util.Vector;

/**
 * claw controls
 * @author robert
 */
public class PhoenixArm {
    public DigitalInput clawTopLimit;
    public DigitalInput clawBottomLimit;
    public SpeedController clawMotor;
    public boolean clawEnabled;
    public AnalogChannel clawPotentiometer;
    public Solenoid clawPneumatic;
    public Vector debugMessages = new Vector(0);
    Const constants;

    public PhoenixArm (SpeedController clawMotor, DigitalInput clawTopLimit,
            DigitalInput clawBottomLimit, Solenoid clawPneumatic,
            AnalogChannel clawPotentiometer, Const constants) {

        this.constants = constants;
        this.clawMotor = clawMotor;
        this.clawTopLimit = clawTopLimit;
        this.clawBottomLimit = clawBottomLimit;
        this.clawPneumatic = clawPneumatic;
        this.clawPotentiometer = clawPotentiometer;
    }

    public void addMessage(String message) {
        debugMessages.addElement(message);
    }

    /**
     * returns potentiometer reading of claw height
     * @return claw height
     */
    public int getHeight() {
        return clawPotentiometer.getAverageValue();
    }

    /**
     * current power setting for claw
     * @return
     */
    public double getClawPower() {
        double clawValue = 0;
        clawValue = (clawMotor != null ? clawMotor.get() : 0);

        return clawValue;
    }

    /**
     * sets raw claw output
     * @param power
     */
    public void setArmPower(double power) {
        double upScale = 9;
        double downScale = .6;
        double resultPower = 0;
        
        if(getLimitBottom() && power < 0) {
            resultPower = 0;
            addMessage("bottom limit exceeded, blocking");
        } else if (getLimitTop() && power > 0) {
            resultPower = 0;
            addMessage("top limit exceeded, blocking");
        } else if (power < 0){
            resultPower = power * downScale;
        } else if (power >= 0) {
            resultPower = power * upScale;
        }

        if(clawMotor != null) clawMotor.set(resultPower);
    }

    private boolean getLimitBottomSoft() {
        if (clawPotentiometer != null) {
            if (getHeight() < constants.CLAWBOTTOMLIMIT()) {
                return true;
            } else {
                return false;
            }
        } else {
            return false;
        }
    }

    /**
     * bottom soft and hard limits
     * @return true if limits hit
     */
    public boolean getLimitBottom() {
        if(!clawBottomLimit.get() || getLimitBottomSoft()) {
            addMessage("Top limits exceeded - blocking");
            return true;
        } else {
            return false;
        }

    }

    private boolean getLimitTopSoft() {
        if (clawPotentiometer != null) {
            if (getHeight() > constants.CLAWTOPLIMIT()) {
                return true;
            } else {
                return false;
            }
        } else {
            return false;
        }
    }

    /**
     * top soft and hard limits
     * @return true if limits hit
     */
    public boolean getLimitTop() {
        if(!clawTopLimit.get() || getLimitTopSoft()) {
            addMessage("Top limits exceeded - blocking");
            return true;
        } else {
            return false;
        }
    }

    /**
     * opens and closes claw
     * @param isEnabled true to open claw, false to close
     */
    public void setClawOpen(boolean isEnabled) {
        clawPneumatic.set(isEnabled);
    }

    /**
     * returns whether claw is open or closed
     * @return
     */
    public boolean getClaw() {
        return clawPneumatic.get();
    }
}
